#include "claw.h"
#include "dji2_0.h"
#include "claw_signal.h"
#include "shooter.h"
#include "claw_motor.h"
#include "../race_tasks/race_tasks.h"

GA_STATUS ga_status = GA_START; //取箭状态

void GA_task(void *argument)
{
    ClawInitHoming();
    GA_start_machine();  //刚开始的状态机
    for (;;)
    {
        if (ga_machine_flag == 1)
            GA_status_machine(); //取箭状态机
        osDelay(5);
    }
}

/*取箭状态机*/
void GA_status_machine()
{
    switch (ga_status)
    {
    case GA_START:
    {
        ClawCtr(0, 0, 0); //抓手全开
        ClawMotorInit();  //电机位置环初始化
        if (ga_mode_flag == 0)
            claw_move_first_pos();
        if (ga_mode_flag == 1)
            claw_move_second_pos();
        osEventFlagsWait(EventTakeArrowHandle, TAKE_ARROW_START, NULL, osWaitForever); //等待取箭
        uprintf("take arrow init done!\r\n");
        ga_status = GA_CATCH;
        break;
    }
    case GA_CATCH:
    {
        uprintf("start catching arrows!\r\n");
        ClawCtr(1, 1, 1);                    //抓手全闭
        osDelay(500);                        // 正在上升
        RaceStateMachine.fetch_arrow_ok = 1; // 给比赛顶层状态机信号
        RaceStateMachine.arrow_blank = 0;
        ga_status = GA_RISE;
        break;
    }
    case GA_RISE:
    {
        uprintf("start rising arrows!\r\n");
        claw_roll_little();
        claw_rise();
        ga_status = GA_ROLL;
        break;
    }
    case GA_ROLL:
    {
        uprintf("start rolling arrows!\r\n");
        claw_roll_homing();
        ga_status = GA_MOVE;
        break;
    }
    case GA_MOVE:
    {
        osEventFlagsWait(EventTakeArrowHandle, CLAW_MOVE, NULL, osWaitForever); //等待移动信号
        if (ga_mode_flag == 0)
        {
            uprintf("start moving arrows(mode1)!\r\n");
            if (loosen_order == 0)
            {
                claw_move_first_arrow(); //移动到第一支箭位置
                ga_status = GA_LOOSEN;
            }
            else if (loosen_order == 1)
            {
                claw_move_second_arrow(); //移动到第二支箭位置
                ga_status = GA_LOOSEN;
            }
            else if (loosen_order == 2)
            {
                claw_move_third_arrow(); //移动到第三支箭位置
                ga_status = GA_LOOSEN;
            }
        }
        else if (ga_mode_flag == 1)
        {
            uprintf("start moving arrows(mode2)!\r\n");
            if (loosen_order == 0)
            {
                claw_move_second_arrow(); //移动到第二支箭位置
                ga_status = GA_LOOSEN;
            }
            else if (loosen_order == 1)
            {
                claw_move_first_arrow(); //移动到第一支箭位置
                ga_status = GA_LOOSEN;
            }
        }

        break;
    }
    case GA_LOOSEN:
    {
        RaceStateMachine.arrow_shooted = 0;
        uprintf("prepare to loosen claw!\r\n");
        claw_drop_little();
        if (loosen_order == 0)
        {
            ga_status = GA_MOVE;
            loosen_order = 1;
            if (ga_mode_flag == 0)
                ClawCtr(0, 1, 1);
            if (ga_mode_flag == 1)
                ClawCtr(1, 0, 1);
            osDelay(500);
        }
        else if (loosen_order == 1)
        {
            ga_status = GA_MOVE;
            loosen_order = 2;
            if (ga_mode_flag == 1)
            {
                ClawCtr(0, 0, 1);
                ga_status = GA_RESET;
                loosen_order = 0;
            }
            if (ga_mode_flag == 0)
                ClawCtr(0, 0, 1);
            osDelay(500);
        }
        else if (loosen_order == 2)
        {
            ga_status = GA_RESET;
            loosen_order = 0;
            ClawCtr(0, 0, 0);
            osDelay(500);
        }
        shooter_load_ready(shooter1);
        claw_rise();
        break;
    }
    case GA_RESET:
    {
        shooter_pause(shooter1);
        uprintf("take arrow reset!\r\n");
        claw_reset();
        RaceStateMachine.arrow_blank = 1; // 箭空，告诉顶层状态机
        ga_status = GA_OVER;
        break;
    }
    case GA_OVER:
    {
        dji_MotorOff(ClawCAN, ClawBoardID, ClawRiseID);
        dji_MotorOff(ClawCAN, ClawBoardID, ClawRollID);
        (ga_mode_flag == 0) ? (ga_mode_flag = 1) : (ga_mode_flag = 0);
        ClawCtr(0, 0, 0);
        osDelay(500);
        ga_status = GA_START;
        break;
    }
    }
}

void GA_start_machine()
{
    uint32_t reset_flag = 0;
    uint32_t evt = 0;
    shooter_restart(shooter1); // 射箭机构开始
    ClawMotorInit();
    ClawCtr(0, 1, 0);
    dji_Homing(ClawCAN, ClawBoardID, ClawRollID, -ClawRollSpeed, 10000); //上翻至限位
    dji_PosCtrl(ClawCAN, ClawBoardID, ClawRiseID, ClawRiseHeight);       //上升
    for (;;)
    {
        evt = osEventFlagsWait(EventTakeArrowHandle, RISE_GET_POSITION | ROLL_FINSIH_HOMING, osFlagsWaitAll, 5000);
        reset_flag = osEventFlagsGet(EventTakeArrowHandle);
        if (evt == (uint32_t)osErrorTimeout)
        {
            if (!(reset_flag & RISE_GET_POSITION))
            {
                dji_PosCtrl(ClawCAN, ClawBoardID, ClawRiseID, ClawRiseHeight);
            }
            if (!(reset_flag & ROLL_FINSIH_HOMING))
            {
                dji_Homing(ClawCAN, ClawBoardID, ClawRollID, -ClawRollSpeed, 10000); //上翻至限位
            }
            continue;
        }
        else if (evt < osStatusReserved && (evt & (RISE_GET_POSITION | ROLL_FINSIH_HOMING)))
        {
            break;
        }
    }
    uprintf("Prepare to place arrow\r\n");
    osEventFlagsWait(EventTakeArrowHandle, CLAW_MOVE, NULL, osWaitForever); //消除第一次的信号
    osEventFlagsWait(EventTakeArrowHandle, CLAW_MOVE, NULL, osWaitForever); //等待移动信号

    uprintf("Claw start move!\r\n");
    claw_move_second_arrow();
    // osEventFlagsWait(EventTakeArrowHandle, CLAW_OPEN, NULL, osWaitForever); //等待松手放箭信号量

    uprintf("Start open claw!\r\n");
    claw_drop_little();
    ClawCtr(0, 0, 0);
    osDelay(500);
    claw_rise();

    uprintf("Reset claw!\r\n");
    claw_reset();
    shooter_pause(shooter1);
    dji_MotorOff(ClawCAN, ClawBoardID, ClawRiseID);
    dji_MotorOff(ClawCAN, ClawBoardID, ClawRollID);
    Signal_CleanClaw();
    osDelay(1000);
}
